extern "C"{
#include "rm_dynamics_arm.h"
}
#include <math.h>
#include <ros/ros.h>
#include <dual_arm_msgs/MoveJ_PO.h>
#include <dual_arm_msgs/Plan_State.h>


using namespace std;


// 接收到订阅的机械臂执行状态消息后，会进入消息回调函数
void planStateCallback(const dual_arm_msgs::Plan_State::ConstPtr &msg)
{
    // 将接收到的消息打印出来，显示是否执行成功
    if(msg->state)
    {
        ROS_INFO("*******Plan State OK");
    } else {
        ROS_INFO("*******Plan State Fail");
    }

}


int main(int argc, char **argv)
{

	// 初始化ROS节点
    ros::init(argc, argv, "forward_kinematics_demo");
    // 创建节点句柄
    ros::NodeHandle nh;
    
	// 声明spinner对象，参数2表示并发线程数，默认处理全局Callback队列
    ros::AsyncSpinner spin(2);
    // 启动两个spinner线程并发执行可用回调 
    spin.start();
    
    // 空间规划指令Publisher
    //ros::Publisher moveJ_PO_left_pub = nh.advertise<dual_arm_msgs::MoveJ_PO>("/l_arm/MoveJ_PO_Cmd", 10);
	ros::Publisher moveJ_PO_right_pub = nh.advertise<dual_arm_msgs::MoveJ_PO>("/r_arm/MoveJ_PO_Cmd", 10);

    
    // 订阅机械臂执行状态话题
    ros::Subscriber planState_sub = nh.subscribe<dual_arm_msgs::Plan_State>("/r_arm/Plan_State", 10, planStateCallback);
    
    RM_Init();
    //机械臂安装角度，右臂为90，-90度；左臂为-90，-90度，输入参数需转换弧度
	//setAngle(-1.57,-1.57); 
	setAngle(90,-90);


    ros::Duration(2.0).sleep();

	//joint记录机械比的目标角度
	float joint[7] = {30,0,0,-75,0,0,0};

	
	//调用运动学正解api，输入为目标位置关节角度，输出为目标位置机械比位姿（位置+欧拉角形式）
	KINEMATIC target_pose;
    target_pose = Forward_Kinematics(joint);
    

	//将目标位姿通过moveJ_P发送给机械臂，控制机械臂运动到目标位姿
	dual_arm_msgs::MoveJ_PO moveJ_PO_target;
	moveJ_PO_target.x = target_pose.pose.x;
	moveJ_PO_target.y = target_pose.pose.y;
	moveJ_PO_target.z = target_pose.pose.z;
	moveJ_PO_target.rx_ = target_pose.ort.rx;
	moveJ_PO_target.ry_ = target_pose.ort.ry;
	moveJ_PO_target.rz_ = target_pose.ort.rz;
	moveJ_PO_target.speed = 0.3;
	
   
	ROS_INFO("forward kinematics target_pose is:[%f, %f, %f, %f, %f, %f]",target_pose.pose.x,target_pose.pose.y,target_pose.pose.z,target_pose.ort.rx,target_pose.ort.ry,target_pose.ort.rz);

	//moveJ_PO_left_pub.publish(moveJ_PO_target);
	moveJ_PO_right_pub.publish(moveJ_PO_target);
    
    ros::waitForShutdown();

    return 0;
}
